#include "ros/ros.h"
#include "move_interface.hpp"
#include "get_state.hpp"

#define PLAN_FIVE_POLY_MODE 0
#define PLAN_CURVE_S_MODE 1
#define PLAN_MOTOR_MODE 2

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "plan_move_motor");
    ros::NodeHandle nh;

    std::vector<double> pos(MOTOR_NUM);
    double time;
    int plan_mode;
    if(argc <= MOTOR_NUM + 1)
    {
        pos[0] = 10.0;
        pos[1] = -10.0;
        pos[2] = -10.0;
        pos[3] = 10.0;
        time = 3.0;
        plan_mode = PLAN_FIVE_POLY_MODE;
    }
    else
    {
        for(unsigned int i = 0; i < MOTOR_NUM; i++)
        {
            pos[i] = std::stod(argv[i + 1]);
        }
        time = std::stod(argv[MOTOR_NUM + 1]);
        plan_mode = std::stoi(argv[MOTOR_NUM + 2]);
    }

    double sample_rate = 200.0;

    if(plan_mode == PLAN_FIVE_POLY_MODE)
    {
        MoveJ(pos, time, sample_rate);
    }
    else if(plan_mode == PLAN_CURVE_S_MODE)
    {
        ROS_ERROR("not impl, only support mode 0 or 2");
    }
    else if(plan_mode == PLAN_MOTOR_MODE)
    {
        MoveMotor(pos);
    }
    else
    {
        ROS_ERROR("not impl, only support mode 0 or 2");
    }

    
    
    ros::shutdown();
}


